﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Shapes;

using NIP.ControllerInterface;


namespace HandEye
{
    /// <summary>
    /// Interaction logic for ManualControl.xaml
    /// </summary>
    public partial class ManualControl : Window
    {
        public int Speed { get; private set; }
        private UsbcController USBC;

        //Here function
        private short herePoint;
        private string here;
        private const short HERE_DIM = 10; //Max number of points to be saved with Here()
        //Saved point
        private Points points;

        public ManualControl()
        {
            InitializeComponent();
            Speed = 50;
            USBC = new UsbcController();
            USBC.Initialization(0, 1);
            Thread.Sleep(2000);
            USBC.Control((byte)'&', true);

            USBC.EnterManual(0);
            //Here function 
            herePoint = 1;
            here = "here";
            USBC.DefineVector((byte)'A', here, HERE_DIM);
            //Saved Point
            //points = (Points)FindResource("pointList"); ;
            points = new Points();
        }

       
        #region Manual controls

        #region All
        private void allButtons_PreviewMouseUp(object sender, MouseButtonEventArgs e)
        {
            try
            {
                int[] pEnc = new int[8];
                int[] pJoint = new int[8];
                int[] pXYZ = new int[8];

                USBC.CloseManual();
                USBC.GetCurrentPosition(pEnc, pJoint, pXYZ);

                tbTorso.Text = pXYZ[0].ToString();
                tbShoulder.Text = pXYZ[1].ToString();
                tbElbow.Text = pXYZ[2].ToString();
                tbPitch.Text = pXYZ[3].ToString();
                tbRoll.Text = pXYZ[4].ToString();
            }
            catch (Exception exception)
            {
                MessageBox.Show(exception.Message);
            }
        }
        #endregion

        #region Torso
        private void btnTorsoPlus_MouseDown(object sender, MouseButtonEventArgs e)
        {
            USBC.EnterManual(0);
            USBC.MoveManual(0, Speed);
        }
        private void btnTorsoMinus_MouseDown(object sender, MouseButtonEventArgs e)
        {
            USBC.EnterManual(0);
            USBC.MoveManual(0, -Speed);
        }
        #endregion

        #region Shoulder
        private void btnShoulderPlus_MouseDown(object sender, MouseButtonEventArgs e)
        {
            USBC.EnterManual(0);
            USBC.MoveManual(1, Speed);
        }
        private void btnShoulderMinus_MouseDown(object sender, MouseButtonEventArgs e)
        {
            USBC.EnterManual(0);
            USBC.MoveManual(1, -Speed);
        }
        #endregion

        #region Elbow
        private void btnElbowPlus_MouseDown(object sender, MouseButtonEventArgs e)
        {
            USBC.EnterManual(0);
            USBC.MoveManual(2, Speed);
        }
        private void btnElbowMinus_MouseDown(object sender, MouseButtonEventArgs e)
        {
            USBC.EnterManual(0);
            USBC.MoveManual(2, -Speed);
        }
        #endregion

        #region Pitch
        private void btnPitchPlus_PreviewMouseDown(object sender, MouseButtonEventArgs e)
        {
            USBC.EnterManual(0);
            USBC.MoveManual(3, Speed);
        }
        private void btnPitchMinus_PreviewMouseDown(object sender, MouseButtonEventArgs e)
        {
            USBC.EnterManual(0);
            USBC.MoveManual(3, -Speed);
        }
        #endregion

        #region Roll
        private void btnRollPlus_MouseDown(object sender, MouseButtonEventArgs e)
        {
            USBC.EnterManual(0);
            USBC.MoveManual(4, Speed);
        }
        private void btnRollMinus_MouseDown(object sender, MouseButtonEventArgs e)
        {
            USBC.EnterManual(0);
            USBC.MoveManual(4, -Speed);
        }
        #endregion

        #region Gripper
        private void btnOpen_Click(object sender, RoutedEventArgs e)
        {
            try
            {
                USBC.OpenGripper();
            }
            catch (Exception exception)
            {
                MessageBox.Show(exception.Message);
            }
        }
        private void btnClose_Click(object sender, RoutedEventArgs e)
        {
            try
            {
                short mm = 0;
                if (USBC.CloseGripper())
                {
                    Thread.Sleep(4000);
                    USBC.GetJaw(ref mm);
                    tbJaw.Text = mm.ToString();
                }
            }
            catch (Exception exception)
            {
                MessageBox.Show(exception.Message);
            }
        }
        #endregion

        #region Belt
        private void btnBeltPlus_PreviewMouseDown(object sender, MouseButtonEventArgs e)
        {
            USBC.EnterManual(0);
            USBC.MoveManual(7, Speed);
        }
        private void btnBeltMinus_PreviewMouseDown(object sender, MouseButtonEventArgs e)
        {
            USBC.EnterManual(0);
            USBC.MoveManual(7, -Speed);
        }
        private void btnBelt_PreviewMouseUp(object sender, MouseButtonEventArgs e)
        {
            USBC.CloseManual();
        }
        #endregion

        private void Grid_PreviewMouseDown(object sender, MouseButtonEventArgs e)
        {
            short mm = 0;
            USBC.GetJaw(ref mm);
            tbJaw.Text = mm.ToString();
        }

        #endregion


        private void btnHere_Click(object sender, RoutedEventArgs e)
        {
            //PointNameWindow window = new PointNameWindow();
            //window.Owner = this;
            //window.ShowDialog();
            String pointName = "here";//window.textBox1.Text;
            if (pointName != null)
            {
                USBC.Here(here, herePoint, -32766);
                int[] pEnc = new int[8];
                int[] pJoint = new int[8];
                int[] pXYZ = new int[8];

                USBC.CloseManual();
                USBC.GetCurrentPosition(pEnc, pJoint, pXYZ);

                points.Add(new SavedPoint(pointName, here, herePoint, pXYZ, pEnc, pJoint));
                herePoint++;
            }
        }

        private void btnMoveToPoint_Click(object sender, RoutedEventArgs e)
        {
            short pointNum = Convert.ToInt16(txtPointNum.Text);
            USBC.MoveLinear(here, pointNum, null, 0);
        }

        private void btnMoveToPos_Click(object sender, RoutedEventArgs e)
        {
            string name = "moveToPos";
            int[] array = { Convert.ToInt32(xPos.Text), Convert.ToInt32(yPos.Text), Convert.ToInt32(zPos.Text), 0, 0 };
            USBC.DefineVector((byte)'A', name, 1);
            USBC.Teach(name, 1, array, 5, -32766);
            USBC.MoveLinear(name, 1, null, 0);
        }

        private void btnMoveToRelPos_Click(object sender, RoutedEventArgs e)
        {
            string relName = "relMove";
            int[] relArray = { Convert.ToInt32(xRelPos.Text), Convert.ToInt32(yRelPos.Text), Convert.ToInt32(zRelPos.Text), 0, 0 };
            USBC.DefineVector((byte)'A', relName, 1);
            USBC.Teach(relName, 1, relArray, 5, -32767);
            USBC.MoveLinear(relName, 1, null, 0);
        }

        //private void btnDoTheMoff_Click(object sender, RoutedEventArgs e)
        //{
        //    Program p = new Program();
        //    p.Add(new LoopStart());
        //    p.Add(new Open());
        //    p.Add(new Close());
        //    p.Add(new LoopEnd());
        //    p.Run();

        //    /*
        //    string vectorName = "moffff";
        //    int[] vectorArray1 = { 339144, 118277, 194789, -84587, 20967 };
        //    int[] vectorArray2 = { 343355, 108790, 114503, -84587, 20967 };
        //    int[] vectorArray3 = { -95676, 307271, 268582, -84623, 21003 };

        //    USBC.DefineVector('A', vectorName, 3);
        //    USBC.Teach(vectorName, 1, vectorArray1, 5, -32766);
        //    USBC.Teach(vectorName, 2, vectorArray2, 5, -32766);
        //    USBC.Teach(vectorName, 3, vectorArray3, 5, -32766);

        //    //Thread implementation
        //    USBC.MoveLinear(vectorName, 1, null, 0);
        //    Thread.Sleep(10000);
        //    USBC.OpenGripper();
        //    Thread.Sleep(3000);
        //    USBC.MoveLinear(vectorName, 2, null, 0);
        //    Thread.Sleep(10000);
        //    USBC.CloseGripper();
        //    Thread.Sleep(3000);
        //    USBC.MoveLinear(vectorName, 1, null, 0);
        //    Thread.Sleep(10000);
        //    USBC.MoveLinear(vectorName, 3, null, 0);
        //    Thread.Sleep(10000);
        //    USBC.OpenGripper();
        //     */
        //}

        private void comboPoints_SelectionChanged(object sender, SelectionChangedEventArgs e)
        {
            //VIRKER IKKE!!
            String name = e.Source.ToString();
            foreach (SavedPoint s in points)
            {
                if(s.pointName == name)
                {
                    USBC.MoveLinear(here, s.pointNumber, null, 0);
                }
            }   
        }

        



    }
}
//TODO release on close window